#include "StompedReality/Math/Matrix.h"
#include "StompedReality/Math/Rectangle.h"
#include "StompedReality/Math/Vector2.h"
#include "StompedReality/collision/CollisionHelper.h"
#include <UnitTest++.h>
#include <iostream>
#define CHECK_VECTOR( v, x, y ) \
  CHECK_CLOSE( x, v->GetX( ), 0.00001 ); \
  CHECK_CLOSE( y, v->GetY( ), 0.00001 );

SUITE( MATH_TESTS ) {

  TEST( TestVector2Base ) {
    StompedReality::Math::Vector2* a = new StompedReality::Math::Vector2( 0.0f, 0.0f );

    CHECK_VECTOR( a, 0, 0 );
    a->SetX( 5 );
    a->SetY( 2 );
    CHECK_VECTOR( a, 5.0f, 2.0f );

    a->SetX( -5.4f );
    a->SetY( 16.78f );
    CHECK_VECTOR( a, -5.4, 16.78f );

    delete a;
  }

  TEST( TestVector2Add ) {
    StompedReality::Math::Vector2* a = new StompedReality::Math::Vector2( 1.0f, 2.0f );
    StompedReality::Math::Vector2* b = new StompedReality::Math::Vector2( 3.0f, 4.0f );

    a->Add( b );

    CHECK_VECTOR( a, 4, 6 );

    b->Add( b );
    CHECK_VECTOR( b, 6, 8 );

    StompedReality::Math::Vector2 x( 1, 2 );
    StompedReality::Math::Vector2 y( 3, 4 );

    x += y;

    CHECK_VECTOR((&x), 4, 6 );

    y = y + y;
    CHECK_VECTOR((&y), 6, 8 );

    delete a;
    delete b;
  }

  TEST( TestVector2Operators ) {
    StompedReality::Math::Vector2 x( 1, 2 );
    StompedReality::Math::Vector2 y( 3, 4 );

    x += y;

    CHECK_VECTOR((&x), 4, 6 );

    y = y + y;
    CHECK_VECTOR((&y), 6, 8 );

    y = y - x;
    CHECK_VECTOR((&y), 2, 2 );

    y = y * 2;
    CHECK_VECTOR((&y), 4, 4 );

    y = y - (x * 0.5f);
    CHECK_VECTOR((&y), 2, 1 );

    x -= y;
    CHECK_VECTOR((&x), 2, 5 );

    x /= 2;
    CHECK_VECTOR((&x), 1, 2.5 );

    x *= 4;
    CHECK_VECTOR((&x), 4, 10 );
  }

  TEST( TestVector2NormAndLength ) {
    StompedReality::Math::Vector2 x( 5, 0 );
    StompedReality::Math::Vector2 y( 0, -5 );

    CHECK_EQUAL( 5, x.Length( ));
    CHECK_EQUAL( 5, y.Length( ));

    x.Normalize( );
    y.Normalize( );

    CHECK_VECTOR((&x), 1, 0 );
    CHECK_VECTOR((&y), 0, -1 );
  }

  TEST( TestRectangleTranslate ) {
    StompedReality::Math::Rectangle x( 50, 50, 50, 50 );

    CHECK_EQUAL( x.Top( ), 0 );
    CHECK_EQUAL( x.Left( ), 0 );
    CHECK_EQUAL( x.Right( ), 100 );
    CHECK_EQUAL( x.Bottom( ), 100 );

    x.Translate( new StompedReality::Math::Vector2( 50, 100 ));

    CHECK_EQUAL( x.Top( ), 100 );
    CHECK_EQUAL( x.Left( ), 50 );
    CHECK_EQUAL( x.Right( ), 150 );
    CHECK_EQUAL( x.Bottom( ), 200 );
  }

  TEST( TestRectangleRectangleContains ) {
    StompedReality::Math::Rectangle x( 50, 50, 50, 50 );
    StompedReality::Math::Rectangle y( 35, 35, 25, 25 );

    CHECK_EQUAL( true, x.Contains ( &y ) );

    CHECK_EQUAL( true, StompedReality::Collision::RectContainsRect( x, y ) );

    x.Translate( new StompedReality::Math::Vector2( 20, 20 ));
    CHECK_EQUAL( false, x.Contains( &y ));
  }

  TEST( TestRectanglePointContains ) {
    StompedReality::Math::Rectangle x( 50, 50, 50, 50 );

    CHECK_EQUAL( true, x.Contains( new StompedReality::Math::Vector2( 10, 10 )));
    CHECK_EQUAL( false, x.Contains( new StompedReality::Math::Vector2( -10, 10 )));

    /*CHECK_EQUAL(true, StompedReality::Math::Rectangle::Contains(new StompedReality::Math::Vector2(0,0),
                new StompedReality::Math::Vector2(100,100), new StompedReality::Math::Vector2(10,10)));
    CHECK_EQUAL(false, StompedReality::Math::Rectangle::Contains(new StompedReality::Math::Vector2(0,0),
                new StompedReality::Math::Vector2(100,100), new StompedReality::Math::Vector2(-10,-10)));*/
  }

  TEST( TestRectangleIntersection ) {
    StompedReality::Math::Rectangle x( 50, 50, 50, 50 );
    StompedReality::Math::Rectangle y( 100, 100, 50, 50 );
    StompedReality::Math::Rectangle z( 0, 0, 50, 50 );

    StompedReality::Math::Rectangle a( -100, 100, 50, 50 );
    StompedReality::Math::Rectangle b( 155, 155, 5, 5 );
    StompedReality::Math::Rectangle c( 15, 15, 5, 5 );

    CHECK_EQUAL( true, x.Instersects( &y ));
    CHECK_EQUAL( true, x.Instersects( &z ));
    CHECK_EQUAL( false, x.Instersects( &a ));
    CHECK_EQUAL( false, x.Instersects( &b ));
    CHECK_EQUAL( true, x.Instersects( &c ));
  }

  TEST( MatrixInitTranslate ) {
    StompedReality::Math::Matrix* m = new StompedReality::Math::Matrix( );

    StompedReality::Math::Matrix* n = StompedReality::Math::Matrix::CreateDM( );

    n->Translate( 50, 100, -0.15f );
    delete m;
    delete n;
  }

  TEST( MatrixMultiB ) {
    StompedReality::Math::Matrix* m = StompedReality::Math::Matrix::CreateTranslationMatrix( 100, 10, 0 );
    StompedReality::Math::Matrix* n = StompedReality::Math::Matrix::CreateTranslationMatrix( 10, 100, 0 );
    StompedReality::Math::Matrix* k = StompedReality::Math::Matrix::CreateTranslationMatrix( 110, 110, 0 );

    m->Multiply( n );

    for (int i = 0; i < 16; i++) {
      CHECK_CLOSE( k->M( )[i], m->M( )[i], 0.00001 );
    }

    delete m;
    delete n;
    delete k;
  }

  TEST( MatrixVectorMultiA ) {
    StompedReality::Math::Matrix* r = StompedReality::Math::Matrix::CreateRotationMatrix( 3.14159265f );
    StompedReality::Math::Matrix* t = StompedReality::Math::Matrix::CreateTranslationMatrix( 0, 100, 0 );

    StompedReality::Math::Vector2* v = new StompedReality::Math::Vector2( 100, 0 );

    StompedReality::Math::Vector2 rt = *v * *t;
    CHECK_VECTOR((&rt), 100, 100 );

    StompedReality::Math::Vector2 rr = *v * *r;
    CHECK_VECTOR((&rr), -100, 0 );

    delete r;
    delete t;
    delete v;
  }

  TEST( MatrixVectorMultiB ) {
    StompedReality::Math::Matrix* r = StompedReality::Math::Matrix::CreateRotationMatrix( 3.14159265f );
    StompedReality::Math::Matrix* t = StompedReality::Math::Matrix::CreateTranslationMatrix( 0, 100, 0 );

    StompedReality::Math::Vector2 rt( 100, 0 );
    rt *= *t;
    CHECK_VECTOR((&rt), 100, 100 );

    StompedReality::Math::Vector2 rr( 100, 0 );
    rr *= *r;
    CHECK_VECTOR((&rr), -100, 0 );

    delete r;
    delete t;
  }

  TEST( VectorDot ) {
    StompedReality::Math::Vector2 a( 3, 2 );
    StompedReality::Math::Vector2 b( 1, 1 );

    CHECK_EQUAL( 5, a.Dot( b ));
  }
}